/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-07     86188       the first version
 */
#include <board.h>
#include <inc/imu.h>
#include <rtdevice.h>
#include <rtthread.h>
#include <rtdbg.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <inc/motor_ctr.h>


rt_device_t imu_dev;
struct serial_configure u5_imu_config = RT_SERIAL_CONFIG_UART5_IMU;
rt_thread_t imu_rxtask;

rt_sem_t imu_rxsem;


void createImuDeviceAndTask(void)
{

    imu_rxtask = rt_thread_create("imu_task",imuRxProTask,0,4096,10,10);
    if(imu_rxtask != RT_NULL)
    {
        rt_thread_startup(imu_rxtask);
        rt_kprintf("imu pro task create secussee..\n");
    }

    imu_rxsem = rt_sem_create("imu_rxsem", 0, RT_IPC_FLAG_PRIO);

    /*  create usart5 device   */

    imu_dev = rt_device_find("uart5");
    if(imu_dev != RT_NULL)
    {
        rt_kprintf("imu uart5 find secussee..\n");

        rt_device_open(imu_dev, RT_DEVICE_FLAG_DMA_RX); //uart_init_rx   if you set dma recivce , use 'RT_DEVICE_FLAG_DMA_RX'
        rt_device_control(imu_dev, RT_DEVICE_CTRL_CONFIG, (void *)&u5_imu_config);
        rt_device_set_rx_indicate(imu_dev, imuCallback);
    }


}


rt_size_t u5_rx_len;

rt_err_t imuCallback(rt_device_t dev, rt_size_t size)
{

    u5_rx_len = size;
    rt_sem_release(imu_rxsem);

    return RT_EOK;
}
void imuRxProTask(void *parameter)
{
    rt_uint8_t imu_buf[256]={0};

    while(1)
    {
        rt_sem_take(imu_rxsem, RT_WAITING_FOREVER);
        rt_device_read(imu_dev, 0, imu_buf, u5_rx_len);
         CopeSerial1Data(imu_buf);
    }
}


float Gyro[3];
float Angle[3];


SAcc    stcAcc;
SGyro   stcGyro;
SAngle  stcAngle;

void jp61Init(void)
{
    //FF AA 67
    float yaw = 100.0f;
    float roll = 100.0f;
    float pitch = 100.0f;
    uint8_t data[] = {0xFF,0xAA,0x67};
    uint16_t time_cnt=0;

    rt_device_write(imu_dev, 0, &data[0], 1);
    rt_device_write(imu_dev, 0, &data[1], 1);
    rt_device_write(imu_dev, 0, &data[2], 1);

    rt_thread_delay(50);
    while(1)
    {
        roll  = (float)stcAngle.Angle[0]/32768*180;
        pitch = (float)stcAngle.Angle[1]/32768*180;
        yaw   = (float)stcAngle.Angle[2]/32768*180;
        time_cnt++;//2s退出
        rt_thread_delay(1);
        if(time_cnt>=2000)
        {
            rt_kprintf("\r\njp61 fail\r\n");
            return;
        }
        if((fabs(yaw)<0.2f) && (fabs(roll)< 0.2f) && (fabs(pitch)<0.2f))
        {
            rt_kprintf("\r\njp61 seccuss\r\n");
            return;
        }
    }
}



void CopeSerial1Data(unsigned char *ucData)
{
    if(ucData[22]!=0x55) //数据头不对，则重新开始寻找0x55数据头
    {
        return;
    }
    else
    {
        switch(ucData[23])//判断数据是哪种数据，然后将其拷贝到对应的结构体中，有些数据包需要通过上位机打开对应的输出后，才能接收到这个数据包的数据
        {
            //memcpy为编译器自带的内存拷贝函数，需引用"string.h"，将接收缓冲区的字符拷贝到数据结构体里面，从而实现数据的解析。
            //case 0x51:    memcpy(&stcAcc,&ucRxBuffer[2],8);break;
//          case 0x52:  memcpy(&stcGyro,&ucRxBuffer[2],8); break; if(actualPos.z < 0 )actualPos.z = 360 + actualPos.z;
            case 0x53:  memcpy(&stcAngle,&ucData[24],8);actualPos.z  = -(float)stcAngle.Angle[2]/32768*180 ; break;
            default : break;

        }
    }
}

